#include <string.h>
#include <FreeRTOS.h>
#include <task.h>
#include "cmd_map_rw.h"
#include "bms_mem_tab.h"
#include "charge_mem_tab.h"

static uint16_t charge_mem[CHARGE_MEM_SIZE];
static uint16_t pwr_mem[BMS_MEM_SIZE];
static uint16_t bms_mem[BMS_MEM_SIZE];

static uint8_t *get_reg_start_addr(CmdMapType type, uint8_t start_index, uint16_t len)
{
    uint8_t *reg_addr = NULL;

    if (len % 2 != 0)
    {
        return NULL;
    }

    switch (type)
    {
    case CMD_MAP_CHARGE:
        if (start_index <= CHARGE_MEM_SIZE-1)
        {
            if (start_index + len/2 > CHARGE_MEM_SIZE)
            {
                return NULL;
            }
            reg_addr = (uint8_t *)&charge_mem[start_index];
        }
        break;
    case CMD_MAP_PWR:
        if (start_index <= (BMS_MEM_SIZE-1))
        {
            if (start_index + len/2 > BMS_MEM_SIZE)
            {
                return NULL;
            }
            reg_addr = (uint8_t *)&pwr_mem[start_index];
        }
        break;
    case CMD_MAP_BMS:
        if (start_index <= (BMS_MEM_SIZE-1))
        {
            if (start_index + len/2 > BMS_MEM_SIZE)
            {
                return NULL;
            }
            reg_addr = (uint8_t *)&bms_mem[start_index];
        }
        break;
    default:
        break;
    }

    return reg_addr;
}

int general_cmd_map_set(CmdMapType type, uint8_t start_index, uint8_t *set_val, uint16_t set_len)
{
    uint8_t *reg_addr = NULL;

    taskENTER_CRITICAL();
    reg_addr = get_reg_start_addr(type, start_index, set_len);
    if (reg_addr == NULL)
    {
        taskEXIT_CRITICAL();
        return -1;
    }

    memcpy(reg_addr, set_val, set_len);
    taskEXIT_CRITICAL();
    return 0;
}

int general_cmd_map_get(CmdMapType type, uint8_t start_index, uint8_t *get_val, uint16_t get_len)
{
    uint8_t *reg_addr = NULL;

    taskENTER_CRITICAL();
    reg_addr = get_reg_start_addr(type, start_index, get_len);
    if (reg_addr == NULL)
    {
        taskEXIT_CRITICAL();
        return -1;
    }

    memcpy(get_val, reg_addr, get_len);
    taskEXIT_CRITICAL();
    return 0;
}

int general_cmd_reg_change_bit(CmdMapType type, uint8_t index, uint16_t bitmask, CmdRegBitChange change)
{
    uint8_t *reg_addr = NULL;
    uint16_t val = 0;
    uint16_t len = 2; // only one reg, 2 bytes
    
    taskENTER_CRITICAL();
    reg_addr = get_reg_start_addr(type, index, len); 
    if (reg_addr == NULL)
    {
        taskEXIT_CRITICAL();
        return -1;
    }

    memcpy(&val, reg_addr, len);
    if (change == CMD_REG_SET_BIT)
    {
        val |= bitmask;
    }
    else
    {
        val &= ~bitmask;
    }
    memcpy(reg_addr, &val, len);
    taskEXIT_CRITICAL();
    return 0;
}

int general_cmd_reg_test_bit(CmdMapType type, uint8_t index, uint16_t bitmask)
{
    uint8_t *reg_addr = NULL;
    uint16_t val = 0;
    uint16_t len = 2; // only one reg, 2 bytes
    int ret = -1;

    taskENTER_CRITICAL();
    reg_addr = get_reg_start_addr(type, index, len); 
    if (reg_addr == NULL)
    {
        taskEXIT_CRITICAL();
        return -1;
    }

    memcpy(&val, reg_addr, len);
    if ((val & bitmask) == bitmask)
    {
        ret = 1;
    }
    else
    {
        ret = 0;
    }
    taskEXIT_CRITICAL();
    return ret;
}

